// 接收端
// 通过监听串口（串口发送A/F+角度）、esp—now两个消息源，控制舵机转动
// 舵机运动需配合PCA9685来控制

#include <Arduino.h>
#include "espNow.hpp"
#include "DaServo.h"

typedef struct
{
  int rece_esp8266;
  int rece_esp32c3;
} Servo;

Servo servo_data;

SemaphoreHandle_t xMutexData = NULL; //创建信号量Handler
TickType_t timeOut = 1000;           //用于获取信号量的Timeout 1000 ticks

// 检查角度是否符合范围
int inline check_angle(int num_in)
{
  if (num_in > 180)
  {
    num_in = 180;
  }
  else if (num_in < 0)
  {
    num_in = 0;
  }
  return num_in;
}

// 收到esp-now时的回调函数
void OnDataRecv(const uint8_t *mac, const uint8_t *incomingData, int len)
{

  if (xSemaphoreTake(xMutexData, timeOut) == pdPASS)
  {
    memcpy(&servo_data, incomingData, sizeof(servo_data));
    //释放钥匙
    xSemaphoreGive(xMutexData);
  }

  Serial.print(F("Bytes received: "));
  Serial.println(len);
}

// 通过串口监听数据
void readCmdTask(void *pvParam)
{

  while (1)
  {
    //获取串口数据
    if (Serial.available() > 0)
    {
      char serialData = Serial.read();
      if (serialData == 'A')
      {
        //检查key并赋值
        if (xSemaphoreTake(xMutexData, timeOut) == pdPASS)
        {
          int angle_tmp = Serial.parseInt();
          servo_data.rece_esp8266 = angle_tmp;
          xSemaphoreGive(xMutexData); //释放钥匙
          Serial.println("A" + angle_tmp);
        }
      }
      else if (serialData == 'F')
      {
        //检查key并赋值
        if (xSemaphoreTake(xMutexData, timeOut) == pdPASS)
        {
          int angle_tmp = Serial.parseInt();
          servo_data.rece_esp32c3 = angle_tmp;
          xSemaphoreGive(xMutexData); //释放钥匙
          Serial.println("F" + angle_tmp);
        }
      }
    }

    vTaskDelay(2);
  }
}
// 通过esp-now监听数据
void readESPTask(void *ptParam)
{
  espBegin();
  servo_data.rece_esp32c3 = 0;
  servo_data.rece_esp32c3 = 0;
  // 注册esp-now消息回调函数
  esp_now_register_recv_cb(OnDataRecv);

  while (1)
  {
    vTaskDelay(200);
  }
}

// 通过servo数据结构体执行动作，每100执行一次
void runCmdTask(void *ptParam)
{
  // 自定义IIC引脚
  //  esp32默认sda为8
  //  esp32默认scl为9
  Wire.setPins(8, 6); // setPins(int sda, int scl)

  unsigned int servoSpeed = 10;
  volatile unsigned target_servo_angle = 0;

  // 舵机初始化
  DaServo myservo1(0, servoSpeed); //建立舵机对象1
  DaServo myservo2(1, servoSpeed); //建立舵机对象2
  DaServo myservo3(2, servoSpeed); //建立舵机对象3
  DaServo myservo4(3, servoSpeed); //建立舵机对象4
  myservo1.init();
  myservo2.init();
  myservo3.init();
  myservo4.init();
  myservo1.setTarget(target_servo_angle); //初始设定舵机1运动目标位置 / myservo1 initial target
  myservo2.setTarget(target_servo_angle); //初始设定舵机2运动目标位置 / myservo2 initial target
  myservo3.setTarget(target_servo_angle); //初始设定舵机3运动目标位置 / myservo3 initial target
  myservo4.setTarget(target_servo_angle); //初始设定舵机4运动目标位置 / myservo4 initial target

  while (1)
  {

    if (xSemaphoreTake(xMutexData, timeOut) == pdPASS)
    {
      target_servo_angle = check_angle(servo_data.rece_esp32c3);

      //释放钥匙
      xSemaphoreGive(xMutexData);
    }

    myservo1.servoUpdate();
    myservo2.servoUpdate();
    myservo3.servoUpdate();
    myservo4.servoUpdate();
    vTaskDelay(100);
  }
}

void setup()
{
  Serial.begin(9600);

  xMutexData = xSemaphoreCreateMutex(); //创建MUTEX

  xTaskCreate(readCmdTask, "readCmd", 1024 * 8, NULL, 1, NULL);
  xTaskCreate(readESPTask, "readEsp", 1024 * 8, NULL, 1, NULL);
  vTaskDelay(1000); //让readCmd提前先运行一秒获取第一笔数据
  xTaskCreate(runCmdTask, "runCmd", 1024 * 8, NULL, 1, NULL);
}

void loop()
{
  vTaskDelay(1000);
}
